/** 
 * University of Plymouth Bioloid Robot Controller
 * for Atmel Mega 128
 * FIRA England Robot Football Team
 * - Based on Dynamixel Evaluation Example (Original Author : Byoung Soo Kim)
 * - Innitial support for executing Motion Editor Pages
 * Authors : 
 * Joerg Wolf, joerg.wolf -xaxtx- plymouth.ac.uk
 * Peter Gibbons, peter.gibbons -xaxtx- 
 * Phill Hall
 * Alexandre Vicente
 *
 * Version 0.21  Updated to include variable frequency and camber. M Mason
 */

#define ENABLE_BIT_DEFINITIONS 
#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/signal.h>
#include <avr/pgmspace.h>
#include <string.h>
#include <stdlib.h>
#include "BioloidBiped.h"
#include "bioloid.h"
#include "robot.h"
#include "motion.h"
#include "comms.h"
//#include "BLC-Protocol.h"
#include "led.h"
#include "AX-12.h"
#include "IMU.h"
#include "PC2CM5.h"


#define DEBUG_MODE


// ---------- extern functions from bioloid-comms.c -------------------
extern void TxDString(unsigned char  *bData);


//extern void MiliSec(word wDelayTime);
extern void CommsInitialize(unsigned char bPort, unsigned char bBaudrate, unsigned char bInterrupt);
extern byte TxPacket(byte bID, byte bInstruction, byte bParameterLength);
extern byte RxPacket(byte bRxLength);

// ---------- extern functions from motion.c --------------
extern void ContinueMotion_HeartBeat(void);
extern void MotionInitialise(void);
extern uint16_t doPose(int page,char pose);
extern uint16_t doPage(int page);

// ----------- extern functions from led.c ---------------
extern void LED_Init(void);
extern void Set_LED(unsigned char LEDno, unsigned char state);

// ------------ Comms Global Variables -------------------
extern volatile byte gbpRxInterruptBuffer[256]; 
extern byte gbpParameter[128];
extern byte gbRxBufferReadPointer;
extern byte gbpRxBuffer[128]; 
extern byte gbpTxBuffer[128]; 

// ------------ Function Prototypes ----------------------
void PortInitialize(void);
void Behaviour_HeartBeat(void);
void Test(void);
void EnableButtons(void);
void ZigbeeHeartBeat(void);


//-------------------Alex_FUnctions----------------
//static int Alex_SetJoint(int JID, int CurrentPosition,int TargetPosition, int Ticks);
int newiabs_2(int i);
//PROGMEM uint8_t my_array[2][2] = { {1,2}, {3,4}};
int HeartBeat_Frequency = 128;

// Number of poses for a half step
// Must enbale number of poses (NumPoses) to be divide by 4 to enable lift and put down for both legs.
// Therefore use either (8, 16, 24, 32, 40, 48, 56, 64, 72, 80, 88, 96, 104, 112, 120 etc..) 



//Variables added by MMason
int NumPoses = 32; 
int Camber = 10;
int SideStepTrip = 0;
int servo_report = 1;
int Foot_Report_Flag=0;
int Foot_ID=122;
int IMU_Report_Flag=0;
int IMU_ID = 120;


IMU_RAW Raw;
// int Step_Frequency = HeartBeat_Frequency / (2 * NumPoses); 	 
							


signed int Right_Hip_Offset = 0,//-48
			Left_Hip_Offset = 0,
			Right_Knee_Offset = 0,//-76
			Left_Knee_Offset = 0,
			Right_Foot_Offset = 0,
			Left_Foot_Offset = 0;//-42

int			Right_Hip,Right_Knee,Right_Foot,Left_Hip,Left_Knee,Left_Foot;
int			Stride_Length = 0,
			Stride_Height = 0,
			Swing = 0,
			Stride_X_offset = 0,	//	Care!  This has been used to test other features
			Stride_Y_offset = 20,
			Ticks_Per_Pose = 2,
			Tilt = 30,
			Turn = 0,
			Right_Leg_Yaw = 0,
			Left_Leg_Yaw = 0,
			Swing_Counter = 0;
			
signed int rx=0,ry=0,lx=0,ly=0;


// ------------ Global Variables -------------------------
volatile byte gbRxBufferWritePointer;
/* RobotFlags is used to indicate interrupts to the main program */
volatile unsigned char RobotFlags;

volatile unsigned char TxBuff[MAXBUF_TX], RxBuff[MAXBUF_RX],Packet[MAXBUF_RX];       // Transmit and Receive buffers
volatile unsigned char *RxPtr, *TxPtr;  					 // Pointers to buffer positions
volatile unsigned int TxBytes; 
volatile unsigned int ROBOTSTATUS;
volatile unsigned char BOB_BOARD_STATUS;
volatile unsigned char BUTTON_STATUS;

int IMUReporting = 0;	//	0: No reporting, 1: Report once, 2: Continuous reporting
int ArmReporting = 0;	//	0: No reporting, 1: Report once, 2: Continuous reporting
int	ZigbeeInstalled = 0;//	0: No Zigbee installed, 1: Zigbee installed, 2: looking for Zigbee
int	ZigbeeCxCount = 0;	//	Counter used to identify no connection on the Zigbee (which is signified by rapidly blinking LED)	
int	ZigbeeConnected = 0;//	1: Zigbee has established a serial link

int	StartPressed = 0;

// ------------- Interrupts ------------------------------

//SIGNAL (SIG_UART0_RECV) in bioloid-comms.c


/**
	Interrupt for START button (INT0)
*/
SIGNAL(SIG_INTERRUPT0)
{
	if( BUTTON_STATUS & (1 << 2)){
		BUTTON_STATUS = 0;
	}else{
		BUTTON_STATUS |= (1 << 0);
	}
	
	StartPressed = 1;
}

SIGNAL(SIG_INTERRUPT1)	//	Zigbee LED output
{
	if (ZigbeeCxCount < 100)
		ZigbeeCxCount ++;
}

/**
	Interrupt for UP button   (INT4)
*/
SIGNAL(SIG_INTERRUPT4)
{
	BUTTON_STATUS |= (1 << 4); 
}
/**
	Interrupt for DOWN button (INT5)
*/
SIGNAL(SIG_INTERRUPT5)
{
	BUTTON_STATUS |= (1 << 5); 
}

/**
	Interrupt for LEFT button (INT6)
*/
SIGNAL(SIG_INTERRUPT6)
{
	BUTTON_STATUS |= (1 << 6); 
}

/**
	Interrupt for RIGHT button (INT7)
*/
SIGNAL(SIG_INTERRUPT7)
{
	BUTTON_STATUS |= (1 << 7); 
}


/**
	HeartBeat of robot at 128 Hz timed in SIGNAL(SIG_OVERFLOW0)
*/
SIGNAL(SIG_OVERFLOW0)
{
	TCNT0=TMRRESET;
	RobotFlags |= _BV(HBEAT);
}

/**
	This interrupt goes off when a byte from the TxBuffer was transmitted.
	If the end of the message has not been reached (TxBytes>0) then the next byte will be transmitted
	to the Pc or PDA
*/
SIGNAL (SIG_UART1_TRANS)
{
	if(TxBytes>0){
		cli();
		UDR1 = *TxPtr;
		TxPtr++;
		TxBytes--;
		if(TxBytes==0){
			TxPtr=TxBuff;
			Set_LED(LED_TXD, LED_OFF);
		}else{Set_LED(LED_TXD, LED_ON);}
		sei();
	}
}

/**
	This interrupt goes off when a byte from the serial port arrives
	the function controls the collection of a data-paket with "[" and "]"
*/
SIGNAL (SIG_UART1_RECV)
{
	*RxPtr = UDR1;
	*(RxPtr+1) = '\0';
	Set_LED(LED_RXD, LED_ON);	//	This gets turned off on the next heartbeat
	
	switch (*RxPtr)
	{
	case '[':
		RxPtr = &RxBuff[0];
		strcpy((char *)RxBuff, "[");
		break;
		
	case ']':
		if (ZigbeeInstalled == 2)	//	then we are looking for a zigbee
		{
			if (strcmp(RxBuff, "[Robotis Zigbee Monitor]") == 0)
				ZigbeeInstalled = 1;
		}
		
		else if (StringIsCompletePacket((char *)RxBuff))
		{
			strcpy((char *)Packet, (char *)RxBuff);
			RobotFlags |= _BV(MSGRCV);
		}
		break;
		
	case '#':
		if (strcmp(RxBuff, "######") == 0)
			exit(0);
		break;
	}
	
	RxPtr++;
	
	if(RxPtr >= &RxBuff[MAXBUF_RX]) // prevent overflow
		RxPtr = &RxBuff[0];
}

unsigned char get_device_data(int ServoNo, unsigned char adr)
{	
	gbpParameter[0] = adr;
	gbpParameter[1] = 1; //Read Length
	TxPacket(ServoNo,INST_READ,2);
	uint8_t bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]);
	if(bRxPacketLength == DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]){
		if(gbpRxBuffer[4]==0){
			return gbpRxBuffer[5];
		}else{
			// error
			return 0x80;
		}
	}else{
		//error
		return 0x80;
	}
}

int UsingPoseSystem = 0;

int	StepNo = 0;

#define START_REVERSE	20

void OnNewStep(void)
{
/*
int			Stride_Length = 0,
			Stride_Height = 0,
			Swing = 0,
			Stride_X_offset = 0,
			Stride_Y_offset = 20,
			Ticks_Per_Pose = 3,
			Tilt = 65,
			Turn = 0,
			Right_Leg_Yaw = 0,
			Left_Leg_Yaw = 0,
			Swing_Counter = 0;
*/
	if (StartPressed)
		CommsPrintf("Step: %d\r\n", ++StepNo);	//	Care! StepNo only gets incremented when Start button has been pressed!
		
	switch(StepNo)
	{
	case 1:
		Swing = 3;
		Stride_Height = 3;
		Tilt = 68;
		break;
		
	case 2:
		Stride_Height = 7;
	case 3:
	case 4:
	case 5:
		Stride_Length += 2;
		break;
		
	case START_REVERSE:
		Stride_Length = 4;
		break;
		
	case START_REVERSE + 1:
		Stride_Length = 0;
		break;
		
	case START_REVERSE + 2:
	case START_REVERSE + 3:
		Stride_Length -= 2;
		Tilt -= 2;
		break;
	}
}


int main(void)
{ 
	int		Tick = 0, 
			i = 0, 
			PoseNum = 0, 
			z = 0;
	
	IMU_DATA		IMUData;
	unsigned short ShoulderFore = 0, ShoulderSplay = 0, Elbow = 0;
	unsigned short ServoPos[JID_L_ANKL_ROLL+1];
	AXIS Axis;
	
	BOB_BOARD_STATUS = 0;
	ROBOTSTATUS = STOP_POSE;
	Balance = BALANCE_OFF;
	BUTTON_STATUS = (1 << 2);

	PortInitialize(); //Port In/Out Direction Definition
	RS485_RXD; //Set RS485 Direction to Input State.
	/* Timer0 is for the HeartBeat */
	RobotFlags = 0;
	TCCR0 = 0x07;  // Use 1024 prescaler on Timer 0 pdf p.105 (this actually sets to 128. For 1024, should be 0x07 with atmega128 
	TIMSK = 0x01;  // Enable timer (overflow interrupt enabled. Same on ATMEGA128) 

	LED_Init();
    Set_LED(LED_POWER,LED_ON);
	Set_LED(LED_MANAGE,LED_ON);
	
	CommsInitialize(SERIAL_PORT0,1,RX_INTERRUPT);//RS485 Initializing(RxInterrupt)
	CommsInitialize(SERIAL_PORT1,DEFAULT_BAUD_RATE,RX_INTERRUPT); //RS232 Initializing(None Interrupt)
	  
	gbRxBufferReadPointer = gbRxBufferWritePointer = 0;  //RS485 RxBuffer Clearing.
	
	sei();  //Enable Interrupt -- Compiler Function
	
	RxPacket(255); //init 1MBit bus
	//InitGyro();
	
	IMUInitialise(0);
		
	/**
		Wait 0.5 seconds and then switch off the MANAGE LED
		This indicates that a reset has occured
	*/
/*	int pause=0;
	do{
		if (RobotFlags & _BV(HBEAT))
		{
			pause++;
			RobotFlags=RobotFlags & ~_BV(HBEAT);
		}
	}while(pause<64);*/
	
	#ifdef DEBUG_MODE	
	TxDString((unsigned char *)"\r\n----------  DEBUG MODE -----------\r\n");
	#endif
	TxDString((unsigned char *)"\r\n[pg00]\r\n[University of Plymouth Bioloid Robot Controller]\r\n");
	
	if(PingDevice(20)){ BOB_BOARD_STATUS |= DEV20_CONNECTED; }
	if(PingDevice(21)){ BOB_BOARD_STATUS |= DEV21_CONNECTED; }
	if(PingDevice(22)){ BOB_BOARD_STATUS |= DEV22_CONNECTED; }
	MotionInitialise();
	
	EnableButtons();
	StartPressed = 0;

	// Example read servo:
	//byte txt[25];
	//sprintf(txt,"pos=%i\r\n",getCurrentServoPos(21)); TxDString(txt);
	
	// Example set servo:
	//SendServoTargetPos(21,512-153,512);	// -45 degrees
	
	
	Set_LED(LED_MANAGE,LED_OFF);
	
	//BBInitialise(GaitReportFn); //Phil's program		
	
	for(;;)
	{
		if (RobotFlags & _BV(MSGRCV))
		{
			cli();
			RobotFlags=RobotFlags & ~_BV(MSGRCV);
			sei();
			

			
			if(CheckProtocol())
				OnGoodPacket();	
		}
		
		
		if (RobotFlags & _BV(HBEAT))
		{
			cli();
			RobotFlags=RobotFlags & ~_BV(HBEAT);
			sei();
			
			Set_LED(LED_RXD, LED_OFF);
			
			//IMUHeartBeat();
			Behaviour_HeartBeat();
			ContinueMotion_HeartBeat();
			ZigbeeHeartBeat();
			//BBHeartBeat();
			
			
			//IMUGetRaw(120,&Raw);
			//CommsPrintf("%d %d %d %d %d %d %d %d %d %d\r\n",(int)Raw.AccelSide,(int)Raw.AccelFore,(int)Raw.AccelVert,(int)Raw.GyroPitch,
			//(int)Raw.GyroRoll,(int)Raw.GyroYaw,(int)Raw.Dig1,(int)Raw.Dig2,(int)Raw.Dig3,(int)Raw.Dig4);
			//IMUGetFiltered(120,&Axis,0,0);
			//CommsPrintf("%d,%d,%d,%d,%d,%d\r\n",(int)Axis.Pitch_Raw,(int)Axis.Pitch,(int)Axis.Roll_Raw,(int)Axis.Roll)
			//CommsPrintf("%d",pgm_read_byte(&my_array[0]));
			/*if (Tick == 640)	//	5 seconds
			{
				CommsPrintf("Starting walk\r\n");
				BBWalkForwards(100, 100,0);
			}*/
			
			//BBWalkForwards(100, 100,0);
			
			/*if (Tick == 3840)	//	30 seconds
			{
				CommsPrintf("Stopping walk\r\n");
				BBStop();
			}*/
			
			
			//---------Alex_Heart_Beat--------
			/*if(Tick==0)
			{
				for(i=0;i<18;i++)
				{
					AX12SetPosition(i+1,pgm_read_word(&Servo[j*18+i]),128);
				}
				j++;
				if(j==32)j=0;
			}*/
		
	
			
			//Stride_Length = 20;
			//Stride_Height = 10;
			
			int SideStepL = 0,
				SideStepR = 0,
				AnkleLiftL = 0,
				AnkleLiftR = 0,
				RightLegBaring = 0;	//	1 when the right leg is carrying the robot
			
			if(Tick==0)
			{
				OnNewStep();
				
				//Do IMU sensor reporting if set
				if (IMU_Report_Flag==1){
				IMUGetRaw(IMU_ID,&Raw);
				CommsPrintf("%d %d %d %d %d %d\r\n",(int)Raw.AccelSide,(int)Raw.AccelVert,(int)Raw.AccelFore,(int)Raw.GyroPitch,
				(int)Raw.GyroRoll,(int)Raw.GyroYaw);
				}
				
				
				
				//Do foot sensor reporting if set
				if (Foot_Report_Flag==1){
				IMUGetRaw(Foot_ID,&Raw);
				CommsPrintf("%d %d %d %d\r\n",(int)Raw.AccelSide,(int)Raw.AccelFore,(int)Raw.AccelVert,(int)Raw.GyroPitch);
				}
				RightLegBaring = (PoseNum >= 0 && PoseNum < NumPoses*1/2)? 1: 0;

				/*if (Stride_X_offset)	//	This is now used as a side step variable. Set - to move left, + to move right.  10 average move, not more than 20.  Tested with step height of 3
				{
					SideStepL += RightLegBaring? -Stride_X_offset:  Stride_X_offset;
					SideStepR += RightLegBaring?  Stride_X_offset: -Stride_X_offset;
				}*/
					
/* Sidestep
				if (Stride_X_offset)
				{
					if (PoseNum >= 0  && PoseNum < NumPoses*1/4)
						AnkleLiftL = (-Stride_X_offset * PoseNum) / 8;
						
					if (PoseNum >= NumPoses*1/4  && PoseNum < NumPoses*2/4)
						AnkleLiftL = (-Stride_X_offset * (NumPoses*2/4 - PoseNum) ) / 8;
					
					if (PoseNum >= NumPoses*2/4 && PoseNum < NumPoses*3/4)
						AnkleLiftR = (Stride_X_offset  * (PoseNum - NumPoses*2/4) ) / 8;
						
					if (PoseNum >= NumPoses*3/4 && PoseNum < NumPoses)
						AnkleLiftR = (Stride_X_offset  * (NumPoses - PoseNum) ) / 8;
				} */
				
				//Hip Yaw COntrol
				if(Turn >= 0)
				{
					if(PoseNum >= 0 && PoseNum < NumPoses/2)
					{
						Right_Leg_Yaw = PoseNum / 2;
						Left_Leg_Yaw = 0;
					}
					
					if(PoseNum >= NumPoses/2 && PoseNum < NumPoses)
					{
						Right_Leg_Yaw = (NumPoses-1 - PoseNum) / 2;
						Left_Leg_Yaw = 0;
					}
					
				}else{
				
					if(PoseNum >= NumPoses/2 && PoseNum < NumPoses)
					{
						Left_Leg_Yaw = (PoseNum - NumPoses/2)/2;
						Right_Leg_Yaw = 0;
					}
				
					if(PoseNum >= 0 && PoseNum < NumPoses/2)
					{
						Left_Leg_Yaw = (NumPoses/2 - 1 - PoseNum)/2;
						Right_Leg_Yaw = 0;
					}
			
				}

					
				if(PoseNum >= 0 && PoseNum < NumPoses*1/4)
				{
					Swing_Counter = PoseNum;
				}
				
				if(PoseNum >= NumPoses*1/4 && PoseNum < NumPoses*2/4)
				{
					Swing_Counter = NumPoses*2/4-1 - PoseNum;
				}
				
				if(PoseNum >= NumPoses*2/4 && PoseNum < NumPoses*3/4)
				{
					Swing_Counter = NumPoses*2/4 - PoseNum;
				}
				
				if(PoseNum >= NumPoses*3/4 && PoseNum < NumPoses)
				{
					Swing_Counter = PoseNum - (NumPoses - 1);
				}
					
					
					
				int Previous_Left_Knee = Left_Knee;
				int Previous_Right_Knee = Right_Knee;
				int Previous_Left_Hip = Left_Hip + Tilt;
				int Previous_Right_Hip = Right_Hip - Tilt;
				int Previous_Left_Foot = Left_Foot;
				int Previous_Right_Foot = Right_Foot;
	
					
				//Right Leg gait generator
				rx = (int)((double)Stride_Length*cos((double)(NumPoses-PoseNum)*(6.2831/(double)NumPoses))+(double)Stride_X_offset);
				ry = (int)((double)Stride_Height*sin((double)(NumPoses-PoseNum)*(6.2831/(double)NumPoses))+(double)Stride_Y_offset);
				
				//Left Leg gait generator
				lx = (int)((double)Stride_Length*cos((double)(NumPoses-PoseNum)*(6.2831/(double)NumPoses)+3.1415)+(double)Stride_X_offset);
				ly = (int)((double)Stride_Height*sin((double)(NumPoses-PoseNum)*(6.2831/(double)NumPoses)+3.1415)+(double)Stride_Y_offset);
					
				if(rx>=0)
				{
					Right_Hip = (int)(pgm_read_word(&Plane[rx*210+ry*3]))-Right_Hip_Offset;
					Right_Knee = (int)(pgm_read_word(&Plane[rx*210+ry*3+1]))-Right_Knee_Offset;
					Right_Foot = (int)(pgm_read_word(&Plane[rx*210+ry*3+2]))+Right_Foot_Offset+AnkleLiftR;
				}else{
					rx*=-1;
					Right_Hip = (int)(pgm_read_word(&NegPlane[rx*210+ry*3]))-Right_Hip_Offset;
					Right_Knee = (int)(pgm_read_word(&NegPlane[rx*210+ry*3+1]))-Right_Knee_Offset;
					Right_Foot = (int)(pgm_read_word(&NegPlane[rx*210+ry*3+2]))+Right_Foot_Offset+AnkleLiftR;
				}
				
				if(lx>=0)
				{
					Left_Hip = 1024-(int)(pgm_read_word(&Plane[lx*210+ly*3]))+Left_Hip_Offset;
					Left_Knee = 1024-(int)(pgm_read_word(&Plane[lx*210+ly*3+1]))+Left_Knee_Offset;
					Left_Foot = 1024-(int)(pgm_read_word(&Plane[lx*210+ly*3+2]))-Left_Foot_Offset+AnkleLiftL;
				}else{
					lx*=-1;
					Left_Hip = 1024-(int)(pgm_read_word(&NegPlane[lx*210+ly*3]))+Left_Hip_Offset;
					Left_Knee = 1024-(int)(pgm_read_word(&NegPlane[lx*210+ly*3+1]))+Left_Knee_Offset;
					Left_Foot = 1024-(int)(pgm_read_word(&NegPlane[lx*210+ly*3+2]))-Left_Foot_Offset+AnkleLiftL;
				}
				
				
				
				int servo_speed;
				int Angle_HipKnee;
				int Angle_KneeFoot;
				int Angle_Hip;
				int Angle_Knee;
				int Angle_Foot;
				
				
				double hip_speed;
				double knee_speed;
				double foot_speed;
				
				Angle_HipKnee = 0;
				Angle_KneeFoot = 0;
				hip_speed = 0;
				knee_speed = 0;
				foot_speed = 0;
				
				servo_speed = 400;
				
				int max_angle = 0;
				
				
				Angle_Hip = newiabs_2(Left_Hip + Tilt - Previous_Left_Hip);
				Angle_Knee = newiabs_2(Left_Knee - Previous_Left_Knee);
				Angle_Foot = newiabs_2(Left_Foot - Previous_Left_Foot);
				
				Angle_HipKnee = newiabs_2(Angle_Hip - Angle_Knee);
				Angle_KneeFoot =  newiabs_2(Angle_Knee - Angle_Foot);
				
				if (Angle_HipKnee > Angle_KneeFoot)
				{	
					max_angle = Angle_HipKnee;
				}
				else
				{
					max_angle = Angle_KneeFoot;
				}
				
				
				hip_speed = 100 + (newiabs_2(512 - (Right_Hip - Tilt) / max_angle)) * 400;
				knee_speed = 100 + (newiabs_2(512 - Right_Knee) / max_angle)  * 400;
				foot_speed = 100 + (newiabs_2(512 - Right_Foot) / max_angle)  * 400;
				
				int Arm_angle;
				
				if(PoseNum >= 0 && PoseNum < NumPoses/2)
					{
						int Arm_angle = max_angle;
					}
				
				if(PoseNum >= NumPoses/2 && PoseNum < NumPoses)
					{
						int Arm_angle = -1*max_angle;						
					}
				
				
				
				if (!UsingPoseSystem)
				{
				/*AX12SetPosition(1,  206 , servo_speed);
				AX12SetPosition(2,  818 , servo_speed);
				AX12SetPosition(3,  252, servo_speed);
				AX12SetPosition(4,  772, servo_speed);
				AX12SetPosition(5,  512, servo_speed);
				AX12SetPosition(6,  512, servo_speed);
				
				*/
				AX12SetPosition(7,  512 + Right_Leg_Yaw * newiabs_2(Turn), servo_speed); //Flipped Signs for new design
				AX12SetPosition(8,  512 - Left_Leg_Yaw * newiabs_2(Turn), servo_speed); //Flipped Signs for new design
				
				
				AX12SetPosition(9,  512 - Swing_Counter * newiabs_2(Swing) + Camber, servo_speed); //Flipped Signs for new design
				AX12SetPosition(10, 512 - Swing_Counter * newiabs_2(Swing) - Camber, servo_speed);//Flipped Signs for new design
				AX12SetPosition(11, 1024 - (Right_Hip - Tilt) , hip_speed);
				AX12SetPosition(12, 1024 - (Left_Hip + Tilt), hip_speed);
				AX12SetPosition(13, 1024 - (Right_Knee), knee_speed);
				AX12SetPosition(14, 1024 - (Left_Knee), knee_speed);
				AX12SetPosition(15, 1024 - (Right_Foot), foot_speed);
				AX12SetPosition(16, 1024 - (Left_Foot) , foot_speed);
				AX12SetPosition(17, 512 + Camber - Swing_Counter * newiabs_2(Swing), servo_speed);//Flipped Signs for new design
				AX12SetPosition(18, 512 - Camber - Swing_Counter * newiabs_2(Swing), servo_speed);//Flipped Signs for new design
				
				AX12SetPosition(19, 512,servo_speed); //New Pelvic Servo
				}
				/*
				if (!UsingPoseSystem)
				{
				AX12SetPosition(1,206,128);
				AX12SetPosition(2,818,128);
				AX12SetPosition(3,252,128);
				AX12SetPosition(4,772,128);
				AX12SetPosition(5,512,128);
				AX12SetPosition(6,512,128);
				AX12SetPosition(7,512+Right_Leg_Yaw*newiabs_2(Turn),255);
				AX12SetPosition(8,512-Left_Leg_Yaw*newiabs_2(Turn),255);
				AX12SetPosition(9, 512 - Swing_Counter * newiabs_2(Swing) - 10 + SideStepR, 255);//the 10 is a leg spread offset
				AX12SetPosition(10,512 - Swing_Counter * newiabs_2(Swing) + 10 + SideStepL, 255);
				AX12SetPosition(11,Right_Hip-Tilt,255);//right hip
				AX12SetPosition(12,Left_Hip+Tilt,255);
				AX12SetPosition(13,Right_Knee,512);//right knee
				AX12SetPosition(14,Left_Knee,512);
				AX12SetPosition(15,Right_Foot,255);//right foot
				AX12SetPosition(16,Left_Foot,255);
				AX12SetPosition(17,512 + Swing_Counter*newiabs_2(Swing) - 10, 255);
				AX12SetPosition(18,512 + Swing_Counter*newiabs_2(Swing) + 10, 255);
				}
				*/
				//CommsPrintf("%d,%d,%d\r\n",f_Right_Hip-Right_Hip,f_Right_Knee-Right_Knee,f_Right_Foot-Right_Foot);
				/*CommsPrintf("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n",	Stride_Length,
																	Stride_Height,
																	Centre_Of_Mass,
																	Stride_X_offset,
																	Stride_Y_offset,
																	Poses_Per_Step,
																	Ticks_Per_Pose,
																	Tilt,
																	Turn_Left,
																	Turn_Right);*/
				PoseNum++;
				if (PoseNum >= NumPoses) PoseNum = 0;

					
			}
		
			
			//-------------------------------
			Tick++;
			if (Tick >= Ticks_Per_Pose) Tick = 0;

			#ifdef DEBUG_MODE
			Test();
			#endif
		}	
	}
}


/** Low level behavior functions can go here 
*/

void Behaviour_HeartBeat(void)
{
	//MeasureGyro();
	static int tick=0;
	static int tick_tilt=0;
	tick++;
	tick_tilt++;
	
	if(tick==6){
		if((Balance)&&(MHD.CurrentPage==-1)){
			int CP[20];
			for(uint8_t ServoNo=13;ServoNo<17;ServoNo++){
				CP[ServoNo]=getCurrentServoPos(ServoNo);
			}
			for(uint8_t ServoNo=13;ServoNo<17;ServoNo++){
			//if((ServoNo==11)||(ServoNo==12)||(ServoNo==13)||(ServoNo==14)){
				int error = ServoPos[ServoNo] - CP[ServoNo];
				int ThetaTarget = ServoPos[ServoNo] + error;
				SendServoTargetPos(ServoNo,ThetaTarget,400);	
			}
		}	
	}
	
	if(tick==6)
	{
		tick=0;
	}
}


void EnableButtons(void)
{
  cli();
  PORTD |= ((1 << 0));
  PORTE |= ((1 << 4)|(1 << 5)|(1 << 6)|(1 << 7)); 
  EIMSK =  (1 << 0)|(1 << 4)|(1 << 5)|(1 << 6)|(1 << 7) | (1 << 1); 
  EICRA = (1<<ISC01) | (1<<ISC11);
  EICRB = (1<<ISC41)|(1<<ISC51)|(1<<ISC61)|(1<<ISC71);
  sei();
}

void PortInitialize(void)
{
  DDRA = DDRB = DDRC = DDRD = DDRE = DDRF = 0;  //Set all port to input direction first.
  PORTB = PORTC = PORTD = PORTE = PORTF = PORTG = 0x00; //PortData initialize to 0
  
  // cbi(SFIOR,2); //All Port Pull Up ready
  SFIOR &= ~_BV(PUD);	//	Make sure the pull-up disable bit of the SFIOR register isn't set
  
  DDRE |= (BIT_RS485_DIRECTION0|BIT_RS485_DIRECTION1); //set output the bit RS485direction

/*  DDRD |= (BIT_ZIGBEE_RESET|BIT_ENABLE_RXD_LINK_PC|BIT_ENABLE_RXD_LINK_ZIGBEE);
  
  PORTD &= ~_BV(BIT_LINK_PLUGIN); // no pull up
  PORTD |= _BV(BIT_ZIGBEE_RESET);
  PORTD |= _BV(BIT_ENABLE_RXD_LINK_PC);
  PORTD |= _BV(BIT_ENABLE_RXD_LINK_ZIGBEE); */
  
	DDRD =  _BV(BIT_ZIGBEE_RESET) | _BV(BIT_ENABLE_RXD_LINK_ZIGBEE) | _BV(BIT_ENABLE_RXD_LINK_PC);
	PORTD = _BV(BIT_ZIGBEE_RESET) | _BV(BIT_ENABLE_RXD_LINK_PC)     | _BV(PD1); // PD1 listens to the Zigbee status LED.  Setting it here sets the internal pull-up;

}

void Sleep(int Heartbeats)
{
	for (int P = 0; P < Heartbeats; )
	{
		if (RobotFlags & _BV(HBEAT))
		{
			RobotFlags &= ~_BV(HBEAT);
			P++;
		}
	}
}


void ZigbeeHeartBeat(void)
{
	static int Tick = 0;
	static int OldConnected = -1;
	
	if (++Tick >= 64)
	{
		Tick = 0;
		
		//	CommsPrintf("Cx:%d, installed:%d, connected:%d\r\n", ZigbeeCxCount, ZigbeeInstalled, ZigbeeConnected);
		
		if (ZigbeeCxCount > 4)
		{
			ZigbeeInstalled = 1;
			ZigbeeConnected = 0;
		}
		else
			ZigbeeConnected = ZigbeeInstalled;	//	i.e. if there's a Zigbee installed then it must be connected.

		ZigbeeCxCount = 0;

		if (ZigbeeConnected != OldConnected)
		{
			PORTD &= ZigbeeConnected? ~_BV(BIT_ENABLE_RXD_LINK_PC):    ~_BV(BIT_ENABLE_RXD_LINK_ZIGBEE);
			PORTD |= ZigbeeConnected?  _BV(BIT_ENABLE_RXD_LINK_ZIGBEE): _BV(BIT_ENABLE_RXD_LINK_PC);
			CommsPrintf(ZigbeeConnected? "Connected via Zigbee\r\n": "Connected via serial port\r\n");
			Set_LED(LED_AUX, ZigbeeConnected? LED_ON: LED_OFF);
		}
		
		OldConnected = ZigbeeConnected;
	}
}



/** 
	Test procedures for debuging 
*/

void ReadServoData(byte ServoID, byte Param0, byte Param1)
{

	gbpParameter[0] = Param0;
	gbpParameter[1] = Param1;
	DebugServoID = ServoID;
	TxPacket(DebugServoID, INST_READ, 2);	//	ServoID, Read data, parameter length
	RxPacket(DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]);
}

void Test(void)
{
	static int tick = 0;
/*	IMU_DATA	IMU;
	extern volatile MOTION_HEARTBEAT_DATA MHD;
	extern volatile int ServoTrim[SERVO_MEMORY];
	unsigned short s1, s2;
	double 	d1,
				ExpPitch = -10 / 180 * 3.14,
				KPP = 307.0 * 0.25;  */
				

return;

	tick++;
	
	if (tick == 128)
	{
		tick = 0;
		CommsPrintf("Tick\r\n");
	}
	
	
return;
	
	if (tick == 64)
		IMUTestMode(1);

return;

	tick ++;
	
	if(tick == 64)
	{
		ROBOTSTATUS = DO_PAGE;
		doPage(1);
	}
	
	if(tick==128*2)
	{
		ROBOTSTATUS = DO_PAGE;
		doPage(44); // Peter's walk is 52, 44 is on spot
	}
	
	//if(tick==128*12)
	//	tick=0;
}


//-------------------------------Alex's_Stuff-------------------------------

int newiabs_2(int i)
{
	return (i < 0)? i * -1: i;
}

static int Alex_SetJoint(int JID, int CurrentPosition,int TargetPosition, int Ticks)//This is Phil's function but modified! The credit is his!
{
	int pos;
	if(CurrentPosition == TargetPosition)return 0;
	unsigned int Dist = newiabs_2(CurrentPosition - TargetPosition);
	unsigned Speed = Dist * SERVO_SPEED_MULTIPLIER;	//	Maximum distance is 1024. 1024 * 56 is < 0xFFFF
	Speed /= Ticks;

	if (Speed < 100)	Speed = 100;
	
	return AX12SetPosition(JID, TargetPosition, Speed);
}

//--------------------------------------------------------------------------